﻿// Author: Jan-Willem Willebrands
// $Revision: 16 $ 
// $Date: 2012-02-10 20:02:53 +0100 (vr, 10 feb 2012) $
// Latest changes: Converted competition code to work with colours again.
// Notes:   This is a fairly optimized version, and as such, some functions have been
//          written out, rather than being used as a function. This applies mainly to
//          histogram updating and median calculations. Moving these calculations to
//          a seperate function increases running time considerately. They do not seem
//          to be getting inlined. Might be worth trying to use the MethodImplOptions.AggressiveInlining
//          in .NET framework 4.5;

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Threading;
using System.Drawing.Imaging;

namespace TLD_Vision.filters
{
	public class MedianFilter : VisionAlgorithm
	{
		private TLD_Vision.Image sourceImage;
		private TLD_Vision.Image outImage;

		private int size = 3;
		public int Size
		{
			get
			{
				return size;
			}
			set
			{
				if(value > 0 && value % 2 != 0)
					size = value;
				else
					throw new Exception("Wrong filter size!");
			}
		}

		public MedianFilter(String Name, int size) : base(Name) {
            this.Size = size;
        }

		public override TLD_Vision.Image DoAlgorithm(TLD_Vision.Image sourceImage)
		{
            int height = sourceImage.Height;
            int width = sourceImage.Width;
            
            outImage = new TLD_Vision.Image(new Bitmap(width, height, PixelFormat.Format32bppArgb));

			// Get the address of the first line.
            IntPtr srcPtr = sourceImage.GetDataPointer();
            IntPtr dstPtr = outImage.GetDataPointer();

            // Multi threading implementation.
            // Signal is used by each thread to notify completion.
            // Image is chopped into several horizontal strips, based on the amount of processors.
			AutoResetEvent signal = new AutoResetEvent(false);

			int firstLine = Size >> 1;
			int lastLine = height - (Size >> 1);

			int numLines = lastLine - firstLine;
			int procCount = 2 * Environment.ProcessorCount;
			int chunk = numLines / procCount;
			int counter = procCount;

			for(int c = 0; c < procCount; c++)
			{
				ThreadPool.QueueUserWorkItem(delegate(Object obj)
				{
					int num = (int)obj;
					int startLine = firstLine + num * chunk;
                    // Number of lines is calculated again for the last chunk.
                    // This is due to rounding error caused by image dimensions that are not
                    // divisible by procCount. The last chunk will be the default chunk size + any leftover lines.
					ProcessData(srcPtr, dstPtr, startLine, width, num == procCount - 1 ? height - startLine - (Size >> 1) : chunk);
					if(Interlocked.Decrement(ref counter) == 0)
					{
						signal.Set();
					}
				}, c);
			}

			signal.WaitOne();

			sourceImage.Unlock();
			outImage.Unlock();

            return outImage;
		}

        
        private void AddToHist(byte[,] hist, uint argb)
        {
            ++hist[0, (byte)argb];
            ++hist[1, (byte)(argb >> 8)];
            ++hist[2, (byte)(argb >> 16)];
        }

        private void RemFromHist(byte[,] hist, uint argb)
        {
            --hist[0, (byte)argb];
            --hist[1, (byte)(argb >> 8)];
            --hist[2, (byte)(argb >> 16)];
        }

        private int MedianFromHist(byte[,] hist, int medianIndex, int clr)
        {
            int sum = 0;
            int index = -1;
            while (sum <= medianIndex)
            {
                sum += hist[clr, ++index];
            }
            
            return index;
        }

		private unsafe void ProcessData(IntPtr src, IntPtr dst, int startLine, int width, int numLines)
		{
            uint* pBase = (uint*)src;
			uint* pDest = (uint*)dst;

			byte[] histR = new byte[256];
            byte[] histG = new byte[256];
            byte[] histB = new byte[256];
            
			int medianIndex = (Size * Size) >> 1;
			int radius = Size >> 1;

			// Initial fill of histograms.
            // We skip the bottom row to integrate nicely with the
            // for loop, which fills the bottom row on each y iteration.
			for(int y = startLine - radius; y < startLine + radius; ++y)
			{
				uint* row = &pBase[y * width];
				for(int x = 0; x < Size; ++x)
				{
                    byte* bgra = (byte*)&row[x];
                    ++histB[bgra[0]];
                    ++histG[bgra[1]];
                    ++histR[bgra[2]];
				}
			}

            // Calculate range up front. Don't know if compiler optimizes this.
			int yDone = startLine + numLines;
			int step = 1;

            // Iterate over each column.
			for(int y = startLine; y < yDone; ++y)
			{
				uint* row = &pBase[y * width];
				int start = step > 0 ? radius : width - 1 - radius;
				int target = step > 0 ? width - radius : radius - 1;

				// Add bottom row of mask to histogram.
				int yAdd = y + radius;
				for(int x1 = start - radius; x1 <= start + radius; ++x1)
				{
                    byte* bgra = (byte*)&pBase[yAdd * width + x1];
                    ++histB[bgra[0]];
                    ++histG[bgra[1]];
                    ++histR[bgra[2]];
				}

                // Iterate over each column. It should be noted that the step
                // variable is used to make a zigzag motion through the pixels.
                // This is done to optimize addition and removal from the histogram,
                // thus never having to make more than 2*Size mutations.
				for(int x = start; x != target; x += step)
				{

                    // Unfortunately we weren't able to remove this check.
                    // At each row, except for the beginning, the histogram is updated
                    // by adding the next row, and removing the last row, i.e. when
                    // moving from left to right, it removes the leftmost row, and adds the
                    // next row to the right.
                    if(x != start)
					{
						int xRem = x - step * (radius + 1);
						int xAdd = x + step * radius;

						for(int y1 = y - radius; y1 <= y + radius; ++y1)
						{
                            byte* bgraRem = (byte*)&pBase[y1 * width + xRem];
                            byte* bgraAdd = (byte*)&pBase[y1 * width + xAdd];

                            ++histB[bgraAdd[0]];
                            ++histG[bgraAdd[1]];
                            ++histR[bgraAdd[2]];

                            --histB[bgraRem[0]];
                            --histG[bgraRem[1]];
                            --histR[bgraRem[2]];
						}
					}

                    // Calculate median values for each color.
                    int sum = 0;
                    int index = -1;
                    while (sum <= medianIndex)
                    {
                        sum += histB[++index];
                    }
                    byte blueMedian = (byte)index;

                    sum = 0;
                    index = -1;
                    while (sum <= medianIndex)
                    {
                        sum += histG[++index];
                    }
                    byte greenMedian = (byte)index;

                    sum = 0;
                    index = -1;
                    while (sum <= medianIndex)
                    {
                        sum += histR[++index];
                    }
                    byte redMedian = (byte)index;

                    // And finally write the median value back to the output file.
                    // Alpha channel is currently not taken into account, and is assumed
                    // fully opaque.
					pDest[y * width + x] = (uint)(0xFF << 24 | redMedian << 16 | greenMedian << 8 | blueMedian);
				}

				// Remove top row of mask from histogram.
				int yRem = y - radius;
				int z = target - step;
				for(int xRem = z - radius; xRem <= z + radius; ++xRem)
				{
                    byte* bgra = (byte*)&pBase[yRem * width + xRem];
                    --histB[bgra[0]];
                    --histG[bgra[1]];
                    --histR[bgra[2]];
				}

                // Invert step, so the next horizontal iteration is performed in the opposite
                // direction to improve performance. See technical documentation for a more detailed
                // description.
				step *= -1;
			}
		}
	}
}
